#include "auto_navigation/planning/global_planner.hpp"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl_conversions/pcl_conversions.h>

namespace auto_navigation {
namespace planning {

GlobalPlanner::GlobalPlanner() 
    : private_nh_("~"), 
      tf_listener_(tf_buffer_), 
      grid_map_received_(false), 
      pointcloud_received_(false),
      has_goal_(false)
{
}

GlobalPlanner::~GlobalPlanner() 
{
    // 停止规划定时器
    if (planning_timer_.isValid()) {
        planning_timer_.stop();
    }
}

bool GlobalPlanner::initialize(ros::NodeHandle& nh) 
{
    nh_ = nh;
    
    // 从参数服务器读取参数
    private_nh_.param<std::string>("map_frame_id", map_frame_id_, "map");
    private_nh_.param<std::string>("base_frame_id", base_frame_id_, "base_link");
    private_nh_.param<double>("plan_frequency", plan_frequency_, 1.0);  // 默认1Hz规划频率
    
    // 初始化规划器管理器
    if (!planner_manager_.initialize(private_nh_)) {
        ROS_ERROR("Failed to initialize planner manager");
        return false;
    }
    
    // 创建并注册A*规划器
    auto astar_planner = std::make_shared<AStarPlanner>();
    if (!planner_manager_.registerPlanner(astar_planner)) {
        ROS_ERROR("Failed to register A* planner");
        return false;
    }
    
    // 创建并注册点云规划器
    auto pointcloud_planner = std::make_shared<PointCloudPlanner>();
    if (!planner_manager_.registerPlanner(pointcloud_planner)) {
        ROS_ERROR("Failed to register PointCloud planner");
        return false;
    }
    
    // 初始化订阅器
    grid_map_sub_ = nh_.subscribe("/map", 1, &GlobalPlanner::gridMapCallback, this);
    pointcloud_sub_ = nh_.subscribe("/cloud_pcd", 1, &GlobalPlanner::pointCloudCallback, this);
    goal_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &GlobalPlanner::goalCallback, this);
    
    // 初始化发布器
    path_pub_ = nh_.advertise<nav_msgs::Path>("/planning/global_path", 1, true);
    vis_path_pub_ = nh_.advertise<nav_msgs::Path>("/planning/global_path_vis", 1, true);
    
    // 启动规划定时器 - 如果启用了持续规划
    if (plan_frequency_ > 0.0) {
        planning_timer_ = nh_.createTimer(ros::Duration(1.0 / plan_frequency_), 
                                        &GlobalPlanner::planningTimerCallback, this);
    }
    
    ROS_INFO("Global planner initialized");
    return true;
}

void GlobalPlanner::setGridMap(const nav_msgs::OccupancyGrid::ConstPtr& map) 
{
    std::lock_guard<std::mutex> lock(map_mutex_);
    
    // 更新栅格地图标志
    grid_map_received_ = true;
    
    // 使用规划管理器设置地图
    planner_manager_.setMap(*map);
    
    ROS_INFO("Grid map updated: %dx%d cells, resolution: %.2f m/cell", 
             map->info.width, map->info.height, map->info.resolution);
}

void GlobalPlanner::setPointCloud(const sensor_msgs::PointCloud2::ConstPtr& cloud) 
{
    std::lock_guard<std::mutex> lock(map_mutex_);
    
    // 更新点云地图标志
    pointcloud_received_ = true;
    
    // 通知规划器管理器点云数据已接收
    planner_manager_.setPointCloudReceived(true);
    
    // 如果有点云规划器，则设置点云地图
    auto pointcloud_planner = dynamic_cast<PointCloudPlanner*>(
        planner_manager_.getPlannerByName("pointcloud_planner").get());
    
    if (pointcloud_planner) {
        pointcloud_planner->setPointCloud(cloud);
        ROS_INFO("Point cloud map updated with %u points", cloud->width * cloud->height);
    } else {
        ROS_WARN("PointCloudPlanner not available, cannot set point cloud");
    }
}

bool GlobalPlanner::planPath(const geometry_msgs::PoseStamped& start, 
                           const geometry_msgs::PoseStamped& goal,
                           nav_msgs::Path& path) 
{
    std::lock_guard<std::mutex> lock(map_mutex_);
    
    // 检查是否有地图数据
    if (!grid_map_received_ && !pointcloud_received_) {
        ROS_ERROR("No map data available for planning");
        return false;
    }
    
    ROS_INFO("Planning path from (%.2f, %.2f) to (%.2f, %.2f)", 
             start.pose.position.x, start.pose.position.y,
             goal.pose.position.x, goal.pose.position.y);
    
    // 根据地图类型选择合适的规划器
    std::string planner_name;
    if (pointcloud_received_) {
        planner_name = "pointcloud_planner";
        ROS_INFO("Using pointcloud planner (pointcloud_received_: %d)", pointcloud_received_);
    } else {
        planner_name = "astar";
        ROS_INFO("Using A* planner (grid_map_received_: %d)", grid_map_received_);
    }
    
    if (!planner_manager_.switchPlanner(planner_name)) {
        ROS_ERROR("Failed to switch to planner %s", planner_name.c_str());
        return false;
    }
    
    // 使用获取的规划器进行验证
    auto active_planner = planner_manager_.getPlannerByName(planner_name);
    if (!active_planner) {
        ROS_ERROR("Failed to get planner by name: %s", planner_name.c_str());
        return false;
    }
    ROS_INFO("Successfully switched to planner: %s", active_planner->getName().c_str());
    
    // 执行路径规划
    ROS_INFO("Planning with '%s'", planner_name.c_str());
    bool success = planner_manager_.plan(start, goal, path);
    if (success) {
        ROS_INFO("Path planning succeeded with %zu points", path.poses.size());
        
        // 发布规划路径
        path_pub_.publish(path);
        vis_path_pub_.publish(path);
    } else {
        ROS_ERROR("Path planning failed");
    }
    
    return success;
}

bool GlobalPlanner::checkGoalValidity(const geometry_msgs::PoseStamped& goal_pose, 
                                    geometry_msgs::PoseStamped& valid_goal_pose) 
{
    std::lock_guard<std::mutex> lock(map_mutex_);
    
    // 检查是否有地图数据
    if (!grid_map_received_ && !pointcloud_received_) {
        ROS_ERROR("No map data available for goal validation");
        return false;
    }
    
    // 初始化返回的有效目标点为输入的目标点
    valid_goal_pose = goal_pose;
    
    // 获取点云规划器（如果使用点云数据）
    PointCloudPlanner* pointcloud_planner = nullptr;
    if (pointcloud_received_) {
        pointcloud_planner = dynamic_cast<PointCloudPlanner*>(
            planner_manager_.getPlannerByName("pointcloud_planner").get());
        
        if (!pointcloud_planner) {
            ROS_ERROR("Failed to get PointCloudPlanner for goal validity check");
            return false;
        }
    } else if (grid_map_received_) {
        // 如果使用栅格地图，可以实现类似的检查
        // 这里暂时假设栅格地图上的目标点是有效的
        ROS_INFO("Using grid map, assuming goal is valid");
        return true;
    } else {
        ROS_ERROR("No map data available");
        return false;
    }
    
    // 检查目标点是否被占用
    int goal_x, goal_y;
    if (!pointcloud_planner->worldToGrid(goal_pose.pose.position.x, goal_pose.pose.position.y, goal_x, goal_y)) {
        ROS_ERROR("Goal position (%.2f, %.2f) is outside the map",
                 goal_pose.pose.position.x, goal_pose.pose.position.y);
        return false;
    }
    
    // 如果目标点没有被占用，那么直接返回有效
    if (!pointcloud_planner->isOccupied(goal_x, goal_y)) {
        ROS_INFO("Goal position (%.2f, %.2f) is valid", 
                goal_pose.pose.position.x, goal_pose.pose.position.y);
        return true;
    }
    
    // 如果目标点被占用，在周围搜索可行的点
    ROS_WARN("Goal position (%.2f, %.2f) is occupied, searching for nearest valid position", 
             goal_pose.pose.position.x, goal_pose.pose.position.y);
    
    // 在逐渐扩大的范围内搜索有效点
    const int max_radius = 20;  // 最大搜索半径（格子数）
    
    for (int radius = 1; radius <= max_radius; ++radius) {
        // 搜索以目标点为中心的正方形区域
        for (int dx = -radius; dx <= radius; ++dx) {
            for (int dy = -radius; dy <= radius; ++dy) {
                // 只检查正方形边界上的点
                if (abs(dx) != radius && abs(dy) != radius) continue;
                
                int check_x = goal_x + dx;
                int check_y = goal_y + dy;
                
                // 检查该点是否有效且未被占用
                if (pointcloud_planner->isValidCell(check_x, check_y) && 
                    !pointcloud_planner->isOccupied(check_x, check_y)) {
                    
                    // 转换回世界坐标
                    double world_x, world_y;
                    pointcloud_planner->gridToWorld(check_x, check_y, world_x, world_y);
                    
                    // 更新有效目标点
                    valid_goal_pose.pose.position.x = world_x;
                    valid_goal_pose.pose.position.y = world_y;
                    
                    ROS_INFO("Found valid goal position at (%.2f, %.2f), %.2f m from original goal",
                             world_x, world_y, 
                             hypot(world_x - goal_pose.pose.position.x, 
                                   world_y - goal_pose.pose.position.y));
                    
                    return true;
                }
            }
        }
    }
    
    ROS_ERROR("Could not find a valid goal position within %d cells of the original goal", max_radius);
    return false;
}

bool GlobalPlanner::getCurrentPose(geometry_msgs::PoseStamped& pose) 
{
    try {
        // 获取从map到base_link的变换
        geometry_msgs::TransformStamped transform_stamped = 
            tf_buffer_.lookupTransform(map_frame_id_, base_frame_id_, ros::Time(0), ros::Duration(1.0));
        
        // 创建位姿
        pose.header.frame_id = map_frame_id_;
        pose.header.stamp = ros::Time::now();
        pose.pose.position.x = transform_stamped.transform.translation.x;
        pose.pose.position.y = transform_stamped.transform.translation.y;
        pose.pose.position.z = transform_stamped.transform.translation.z;
        pose.pose.orientation = transform_stamped.transform.rotation;
        
        return true;
    } catch (tf2::TransformException &ex) {
        ROS_WARN("Failed to get transform: %s", ex.what());
        return false;
    }
}

void GlobalPlanner::gridMapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg) 
{
    setGridMap(msg);
}

void GlobalPlanner::pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{
    setPointCloud(msg);
}

void GlobalPlanner::goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) 
{
    ROS_INFO("Received goal: (%.2f, %.2f)", msg->pose.position.x, msg->pose.position.y);
    
    // 检查目标点的可行性
    geometry_msgs::PoseStamped valid_goal;
    if (!checkGoalValidity(*msg, valid_goal)) {
        ROS_ERROR("Goal validation failed, rejecting goal");
        return;
    }
    
    // 如果目标点有变化，输出提示信息
    if (fabs(valid_goal.pose.position.x - msg->pose.position.x) > 0.01 || 
        fabs(valid_goal.pose.position.y - msg->pose.position.y) > 0.01) {
        ROS_WARN("Original goal at (%.2f, %.2f) was occupied, using modified goal at (%.2f, %.2f)",
                msg->pose.position.x, msg->pose.position.y,
                valid_goal.pose.position.x, valid_goal.pose.position.y);
    }
    
    // 保存当前目标点（已验证为可行的点）
    current_goal_ = valid_goal;
    has_goal_ = true;
    
    // 发布新的有效目标点
    // 这个步骤可选，但对于调试很有用
    geometry_msgs::PoseStamped valid_goal_msg = valid_goal;
    valid_goal_msg.header.stamp = ros::Time::now();
    if (!valid_goal_pub_) {
        valid_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/planning/valid_goal", 1, true);
    }
    valid_goal_pub_.publish(valid_goal_msg);
    
    // 如果未启用持续规划(频率为0)，则立即规划一次
    if (plan_frequency_ <= 0.0) {
        // 获取当前位姿
        geometry_msgs::PoseStamped current_pose;
        if (!getCurrentPose(current_pose)) {
            ROS_ERROR("Failed to get current pose, cannot plan path");
            return;
        }
        
        // 规划路径
        nav_msgs::Path path;
        planPath(current_pose, current_goal_, path);
    }
}

void GlobalPlanner::planningTimerCallback(const ros::TimerEvent& event) 
{
    // 如果没有目标点，不进行规划
    if (!has_goal_) {
        return;
    }
    
    // 获取当前位姿
    geometry_msgs::PoseStamped current_pose;
    if (!getCurrentPose(current_pose)) {
        ROS_WARN_THROTTLE(5, "Failed to get current pose, skipping this planning cycle");
        return;
    }
    
    // 规划路径
    nav_msgs::Path path;
    planPath(current_pose, current_goal_, path);
}

} // namespace planning
} // namespace auto_navigation 